#pragma once

#include <vector>
#include <random>

#include "Formation.h"
#include "assign2d.h"

#include "UpdateObject.h"
#include <dynamic_reconfigure/server.h>


namespace iusc
{
    struct UGVParams{
        double x,y,z;  // 车的位置
        double vx= 0,vy = 0 ;  // 车的速度
        double ax= 0,ay = 0;  // 控制量
        std::string link;  // 对应的link名称
        bool landed;  // 是否可以降落
        // std::string color;
    };

    struct UGVFormationParams{
        int num = 10;  // UGV数目
        int formationType;  // 编队类型，待定
        int moveType;  // 运动模态
        double distance;
        double vmax;
        double vx,vy;  // 编队的运行速度
        double x,y,z;  // 编队当前的中心点
    };

    class UGV : public UpdateObject{  // 单个UGV
        public:
            using Ptr = std::shared_ptr<UGV>;

            public:
            void initialize() override final;
            void update(double dt) override final;
            void getPose(geometry_msgs::PoseStamped& pose) const override final;

            void setParam(const UGVParams& param){
                m_param = param;
            };
            void getParam(UGVParams& param) const{
                param = m_param;
            };

            std::string ID() const {return m_param.link;};
            bool Landed() const {return m_param.landed;};
        
        private:
            UGVParams m_param; 
            double x,y,z;  // 当前位置
            

    };



    class UGVFormation : public UpdateObject{  // UGV编队
        public:
            using Ptr = std::shared_ptr<UGVFormation>;
            void initialize() override final;
            void update(double dt) override final;
            void getPose(geometry_msgs::PoseStamped& pose) const override final;

            bool addUGV(UGV::Ptr ugv);  // 增加UGV
            void getAllUGVs(std::vector<UGV::Ptr>& ugvs){ugvs = m_ugvObjects;}

            void setParam(const UGVFormationParams& param){
                m_formationParams = param;
                m_formationInitX = param.x;  // 这里写这个不太好，但是懒得改了
                m_formationInitY = param.y;
            }
            void getParam(UGVFormationParams& param) const{
                param = m_formationParams;
            }
            void setUGVInitPoses(const std::vector<formation::Point>& poses){
                m_initializeUGVPoses = poses; 
                for (int i=0;i<m_ugvObjects.size();i++){
                    auto ugv = m_ugvObjects[i];
                    UGVParams p;
                    ugv->getParam(p);
                    p.x = m_initializeUGVPoses[i].x;
                    p.y = m_initializeUGVPoses[i].y;
                    p.z = m_initializeUGVPoses[i].z;
                    p.vx = 0;
                    p.vy = 0;
                    p.ax = 0;
                    p.ay = 0;
                    ugv->setParam(p);
                }
            }

            void getGroundTruth(std::vector<geometry_msgs::PoseStamped>& ugvPoses) const;  // 获取真值

            void reset(UGVFormationParams& param);  // 重置到初始状态
            void start();  // 开始运动
            void stop();  // 不更新了
            void Continue(){m_stopFlag = false;}

            void loadFormationConfig(const std::string& filename);  // 从文件中加载
            

        protected:

            void updateStage1(double dt);  // 第一阶段的更新
            void assignUGVs();  // 分配无人车
            void moveStage1UGVs(double dt);
            bool judgeStage1Finished();  // 判断第一阶段是否完成更新

            void updateStage2(double dt);  // 第二阶段的更新
            bool checkCollision(double x,double y,double vx,double vy,double dt,double threshold,double& dis);  

        private:
            bool m_enableFlag = false;  // 是否开始运动
            bool m_stage1Flag= true;  // 是否进行第一阶段，否则进行第二阶段
            bool m_stopFlag = false;

            UGVFormationParams m_formationParams;
            // 初始化时确定了的
            std::vector<UGV::Ptr> m_ugvObjects;  // 存储ugv
            std::vector<UGV::Ptr> m_randomUGVs;
            double m_formationInitX,m_formationInitY;
            std::vector<formation::Point> m_initializeUGVPoses;  // 无人车初始位置，从gazebo中获取

            // 编队队形相关
            std::map<int,UGV::Ptr> m_formationMap;  // 哪个无人车对应的编队的索引
            std::vector<formation::Point> m_formationRelPoses;  // 编队的相对位置
            // 编队运动规律相关
            double m_totalTrajTime = 0.0;
            double m_trajExecTime = 0.0;  // 仅在第二阶段有用
            std::vector<double> m_trajT;  // 执行轨迹速度段的时间
            std::vector<formation::Point> m_trajVel;  // 执行轨迹的速度
            // geometry_msgs::PoseStamped m_formationCenter;  // 编队的中心位置
    };
}